/*
 * CANJaguar1405.cpp
 *
 *  Created on: Mar 18, 2013
 *      Author: trwulff
 */
#ifndef CANJAGUAR1405_CPP
#define CANJAGUAR1405_CPP
#include "CANJaguar1405.hpp"

/*
 * Default constructor
 */
CANJaguar1405::CANJaguar1405(UINT8 deviceNumber, ControlMode controlMode)
:CANJaguar(deviceNumber)
{
	if(controlMode==kPercentCurrent)
		{
		CANJaguar::ChangeControlMode(CANJaguar::kCurrent);
		}
	else if(controlMode==kPercentVI)
		{
		CANJaguar::ChangeControlMode(CANJaguar::kPercentVbus);
		}
	else CANJaguar::ChangeControlMode((CANJaguar::ControlMode)controlMode);
	MyControlMode = controlMode;
	mP = 0.0;
	mI = 0.0;
	mD = 0.0;
	InitCount=0;
	DeviceID = "("+ConvertItoA(deviceNumber)+")";
	InitSMDFlg=false;
	IsOverCurrent=false;
	IsLoggingOn=false;	
	SmartDashboard::init();
/*
	SmartDashboard::PutNumber("MyControlMode",MyControlMode); // TODO: Modify to include device #
	SmartDashboard::PutNumber("InitCount",InitCount);// TODO: Modify to include device #
	SmartDashboard::PutBoolean("Logging Check",false);
*/
}

/*
 * Default destructor
 */
CANJaguar1405::~CANJaguar1405()
{
	
}
void CANJaguar1405::SetPID (double p, double i, double d)
{
	mP = p;
	mI = i;
	mD = d;
	CANJaguar::SetPID ( mP,  mI,  mD);
	/*
	SmartDashboard::PutNumber("P"+DeviceID,mP);
	SmartDashboard::PutNumber("I"+DeviceID,mI);
	SmartDashboard::PutNumber("D"+DeviceID,mD);
*/
}
void CANJaguar1405::Init()
{
	InitCount++;
	CANJaguar::SetPID(mP, mI, mD);
	CANJaguar::ChangeControlMode ((CANJaguar::ControlMode)MyControlMode);
	CANJaguar::EnableControl();
	SmartDashboard::PutNumber("MyControlMode",MyControlMode);
	SmartDashboard::PutNumber("InitCount",InitCount);
}
void CANJaguar1405::ChangeControlMode (ControlMode controlMode)
{
	MyControlMode = controlMode;
	if(controlMode==kPercentCurrent)
		{
		CANJaguar::ChangeControlMode(CANJaguar::kCurrent);
		}
	else if(controlMode==kPercentVI)
		{
		CANJaguar::ChangeControlMode(CANJaguar::kPercentVbus);
		}
	else CANJaguar::ChangeControlMode((CANJaguar::ControlMode)controlMode);
	SmartDashboard::PutNumber("MyControlMode",MyControlMode);

}
void CANJaguar1405::Set (float value, UINT8 syncGroup)
{
	if (MyControlMode != (CANJaguar1405::ControlMode) CANJaguar::GetControlMode())Init();
//		this->ConfigFaultTime(0.5); // TODO: fix for general purpose
		if ((MyControlMode==kPercentCurrent)||(MyControlMode==kPercentVI))value =value*PERCENT_CURRENT_FACTOR;
		if(MyControlMode==kPercentVI)VIControl();
/*
		SetLog(SmartDashboard::GetBoolean("Logging Check"));
		if(IsLoggingOn)
		{
			SmartDashboard::PutNumber("Time"+DeviceID,LogTimer.Get());	
			SmartDashboard::PutNumber("Control Mode"+DeviceID,CANJaguar::GetControlMode());
			SmartDashboard::PutNumber("Current"+DeviceID,CANJaguar::GetOutputCurrent());
			SmartDashboard::PutNumber("Voltage"+DeviceID,CANJaguar::GetOutputVoltage());
			SmartDashboard::PutNumber("Faults"+DeviceID,CANJaguar::GetFaults());
			SmartDashboard::PutNumber("Bus Voltage"+DeviceID,CANJaguar::GetBusVoltage());
			SmartDashboard::PutNumber("Temperature"+DeviceID,CANJaguar::GetTemperature());
			SmartDashboard::PutNumber("Power Cycle Fault"+DeviceID,CANJaguar::GetPowerCycled ());
			SmartDashboard::PutNumber("P"+DeviceID,CANJaguar::GetP());
			SmartDashboard::PutNumber("I"+DeviceID,CANJaguar::GetI());
			SmartDashboard::PutNumber("D"+DeviceID,CANJaguar::GetD());
			
		}
		SmartDashboard::PutNumber("Set Value"+DeviceID,value);
*/
		CANJaguar::Set (value, syncGroup=0);
//		SmartDashboard::PutNumber("InitCount",InitCount);

}

bool CANJaguar1405::Calibrate() // TODO: add calibration technique -include x axis time
{
/*
	mP = SmartDashboard::GetNumber("P"+DeviceID);
	mI = SmartDashboard::GetNumber("I"+DeviceID);
	mD = SmartDashboard::GetNumber("D"+DeviceID);
	CANJaguar1405::SetPID(mP, mI, mD);
	SmartDashboard::PutNumber("Current"+DeviceID, CANJaguar::GetOutputCurrent());
*/
	return false;
}

void CANJaguar1405::InitSMD()
{
/*
	if(!InitSMDFlg)
	{
	SmartDashboard::PutNumber("P"+DeviceID,mP);
	SmartDashboard::PutNumber("I"+DeviceID,mI);
	SmartDashboard::PutNumber("D"+DeviceID,mD);
	SmartDashboard::PutNumber("Current"+DeviceID, CANJaguar::GetOutputCurrent());
	InitSMDFlg=true;
	}
*/
}
void CANJaguar1405::Status()// TODO: add status
{
	
}

string CANJaguar1405::ConvertItoA(int deviceNumber)
{
	string ReturnString="";
	int temp1=deviceNumber,temp2;
	if(temp1==0)return "0";
	while(temp1!=0)
	{
		temp2=temp1-(temp1/10)*10;
		
		switch (temp2)
		{
		case 0:
			ReturnString="0"+ReturnString;
			break;

		case 1:
			ReturnString="1"+ReturnString;
			break;
			
		case 2:
			ReturnString="2"+ReturnString;
			break;
			
		case 3:
			ReturnString="3"+ReturnString;
			break;
			
		case 4:
			ReturnString="4"+ReturnString;
			break;

		case 5:
			ReturnString="5"+ReturnString;
			break;

		case 6:
			ReturnString="6"+ReturnString;
			break;

		case 7:
			ReturnString="7"+ReturnString;
			break;

		case 8:
			ReturnString="8"+ReturnString;
			break;

		case 9:
			ReturnString="9"+ReturnString;
			break;
		

		}
		temp1=temp1/10;

		
	}
			return ReturnString;
}

void CANJaguar1405::VIControl()
{
	if(!IsOverCurrent)if (	
							((CANJaguar::GetOutputCurrent()>=OVERCURRENT_TRIP_CURRENT)&&
							(CANJaguar::GetOutputCurrent()<=OVERCURRENT_RESET_CURRENT))||
							((CANJaguar::GetOutputCurrent()<= -OVERCURRENT_TRIP_CURRENT)&&
							(CANJaguar::GetOutputCurrent()>= -OVERCURRENT_RESET_CURRENT))
						)
		{
		SmartDashboard::PutString("SMODE"+DeviceID,"Current Mode");
		CANJaguar::ChangeControlMode(CANJaguar::kCurrent); 
		IsOverCurrent=true;
		}
	else
		{
		SmartDashboard::PutString("SMODE"+DeviceID,"Voltage Mode");
		CANJaguar::ChangeControlMode(CANJaguar::kPercentVbus); 
		IsOverCurrent=false;
		}
}

void CANJaguar1405::SetLog(bool flag)
{
/*
	IsLoggingOn=flag;
	SmartDashboard::PutBoolean("Logging",flag);
	if(IsLoggingOn)
	{
		if(!IsLogTimerOn)
		{
				LogTimer.Reset();
				LogTimer.Start();
				IsLogTimerOn=true;
				SmartDashboard::PutNumber("Firmware Version"+DeviceID,CANJaguar::GetFirmwareVersion());
				SmartDashboard::PutNumber("Hardware Version"+DeviceID,CANJaguar::GetHardwareVersion());

		}
	}
	else
	{
		if(IsLogTimerOn)
		{
				LogTimer.Stop();
				IsLogTimerOn=false;
		}
	
	}
*/
}
#endif
